#include "point2pixel_fish.h"

Point2PixelFish::Point2PixelFish(): Point2Pixel()
{
}

Point2PixelFish::~Point2PixelFish(){
}

cv::Point2d Point2PixelFish::loop(const pcl::PointXYZI& point) {
    //外参
    pcl::PointXYZI pt;
    pt = pcl::transformPoint(point, transform_A);

    // 按照线性模型，归一化(f=1)图像坐标系
    double tmpxC = pt.x / pt.z;
    double tmpyC = pt.y / pt.z;
    cv::Point2d planepointsC;

    double r1 = pow((tmpxC * tmpxC + tmpyC * tmpyC), 0.5);//归一化平面的极坐标轴长
    double a0 = std::atan(r1);//点和光轴的夹角
    double a1 = a0 * (1 + distCoeffs.at<double>(0) * pow(a0, 2) +
                      distCoeffs.at<double>(1) * pow(a0, 4)
                      + distCoeffs.at<double>(2) * pow(a0, 6) +
                      distCoeffs.at<double>(3) * pow(a0, 8));//畸变矫正，因镜头误差导致的新夹角，根据等距模型，也等于成像平面的矫正后新轴距
    //按照相似三角形计算矫正后的归一化平面点的坐标
    planepointsC.x = (a1 / r1) * tmpxC;
    planepointsC.y = (a1 / r1) * tmpyC;
    //计算像素坐标
    planepointsC.x = cameraMatrix.at<double>(0, 0) * planepointsC.x + cameraMatrix.at<double>(0, 2);
    planepointsC.y = cameraMatrix.at<double>(1, 1) * planepointsC.y + cameraMatrix.at<double>(1, 2);

    return planepointsC;
}


